﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Microsoft.Kinect;
using System.Windows.Media;
using System.Windows.Controls;
using System.Windows;
using System.Windows.Shapes;

namespace GestureSaver
{
    public static class SkeletonDrawer
    {
        public static void DrawSkeleton(Skeleton skeleton, Brush brush, Canvas canvas, KinectSensor sensor)
        {
            canvas.Children.Clear();

            //Draw bones
            canvas.Children.Add(GetBodySegment(skeleton.Joints, brush, sensor, JointType.HipCenter, JointType.Spine, JointType.ShoulderCenter, JointType.Head));
            canvas.Children.Add(GetBodySegment(skeleton.Joints, brush, sensor, JointType.ShoulderCenter, JointType.ShoulderLeft, JointType.ElbowLeft, JointType.WristLeft, JointType.HandLeft));
            canvas.Children.Add(GetBodySegment(skeleton.Joints, brush, sensor, JointType.ShoulderCenter, JointType.ShoulderRight, JointType.ElbowRight, JointType.WristRight, JointType.HandRight));
            canvas.Children.Add(GetBodySegment(skeleton.Joints, brush, sensor, JointType.HipCenter, JointType.HipLeft, JointType.KneeLeft, JointType.AnkleLeft, JointType.FootLeft));
            canvas.Children.Add(GetBodySegment(skeleton.Joints, brush, sensor, JointType.HipCenter, JointType.HipRight, JointType.KneeRight, JointType.AnkleRight, JointType.FootRight));

            //Draw points
            foreach (Joint j in skeleton.Joints)
            {
                Point mappedPoint = GetDisplayPosition(j.Position, sensor);

                Ellipse ellipse = new Ellipse
                {
                    Fill = Brushes.Green,
                    Width = 10,
                    Height = 10,
                    Margin = new Thickness(mappedPoint.X, mappedPoint.Y, 0, 0)
                };

                canvas.Children.Add(ellipse);
            }
        }

        private static Polyline GetBodySegment(JointCollection joints, Brush brush, KinectSensor sensor, params JointType[] ids)
        {
            PointCollection points = new PointCollection(ids.Length);
            for (int i = 0; i < ids.Length; ++i)
            {
                points.Add(GetDisplayPosition(joints[ids[i]].Position, sensor));
            }
            Polyline polyline = new Polyline();
            polyline.Points = points;
            polyline.Stroke = brush;
            polyline.StrokeThickness = 2;
            return polyline;
        }

        private static Point GetDisplayPosition(SkeletonPoint point, KinectSensor sensor)
        {
            Point rVal = new Point();
            
            if (sensor.DepthStream.IsEnabled)
            {
                DepthImagePoint p = sensor.MapSkeletonPointToDepth(point, DepthImageFormat.Resolution640x480Fps30);
                rVal.X = p.X;
                rVal.Y = p.Y;
            }
            else
            {
                ColorImagePoint p = sensor.MapSkeletonPointToColor(point, ColorImageFormat.RgbResolution640x480Fps30);
                rVal.X = p.X;
                rVal.Y = p.Y;
            }

            return rVal;
        }
    }
}
